package com.hrg.rds.infra.executor.impl;

import cn.hutool.core.collection.CollUtil;
import cn.hutool.core.util.IdUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.json.JSONUtil;
import com.hrg.rds.biz.comm.MiscService;
import com.hrg.rds.biz.comm.NavService;
import com.hrg.rds.biz.comm.StatusService;
import com.hrg.rds.biz.config.RdsConfiguration;
import com.hrg.rds.biz.robot.domain.*;
import com.hrg.rds.biz.traffic.TrafficManagement;
import com.hrg.rds.biz.waybill.domain.instance.AgvMsg;
import com.hrg.rds.biz.waybill.domain.template.cmd.CmdParam;
import com.hrg.rds.biz.waybill.service.AgvMsgService;
import com.hrg.rds.infra.executor.CmdExecutor;
import com.hrg.rds.infra.executor.CmdInstanceContext;
import com.hrg.rds.infra.executor.CmdInstanceResult;
import com.hrg.seer.sdk.rbk.api.DIOOperationType;
import com.hrg.seer.sdk.rbk.api.JackOperationType;
import com.hrg.seer.sdk.rbk.api.RollerOperationType;
import com.hrg.seer.sdk.rbk.msg.nav.*;
import com.hrg.seer.sdk.rbk.msg.status.Task;
import com.hrg.seer.sdk.rbk.msg.status.TaskStatusPackage;
import com.hrg.sys.common.constant.UserConstants;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import java.time.Duration;
import java.util.ArrayList;
import java.util.Date;
import java.util.List;
import java.util.Map;
import java.util.function.Function;
import java.util.stream.Collectors;

@Slf4j
@Service("AgvNavOperationCmd")
public class AgvNavOperationCmdExecutor implements CmdExecutor {
	@Autowired
	private AgvMsgService agvMsgService;

	@Override
	public Duration getRetryInterval() {
		return Duration.ofSeconds(1);
	}

	@Override
	public CmdInstanceResult execute(List<CmdParam> params, CmdInstanceContext cmdInstanceContext) {
		log.info("AgvNavOperationCmdExecutor execute, cmdId = "+ cmdInstanceContext.getCmdId());
		try {
			AgvVo agvVo = cmdInstanceContext.getAgvVo();
			if (!agvVo.isIfAvailable4Current()) {
				log.info("agv[{}] is not available", agvVo.getIp());
				return CmdInstanceResult.retry();
			}

			Map<String, CmdParam> map = params.stream().collect(Collectors.toMap(CmdParam::getKey, Function.identity()));
			String targetStation = map.get("targetStation").getEvaluatedValue();
			// 是否只占用到当前站点
			boolean onlyOccupiedToCurrent = false;
			if (map.containsKey("onlyOccupiedToCurrent")) {
				onlyOccupiedToCurrent = map.get("onlyOccupiedToCurrent").getBooleanEvaluatedValue();
			}
			String operation = map.get("operation").getEvaluatedValue();
			AgvType type = agvVo.getType();
			// 检查是否已经发送过导航命令
			AgvMsg agvMsg = cmdInstanceContext.getLastAgvMsg();

			// 辊筒车 // 包含jack
			boolean onlyJackSetHeight = false;
			boolean rollByDIO = false;
			Double height = null;
			RollerOperationType rollerOperationType = null;
			JackOperationType jackOperationType = null;
			DIOOperationType dioOperationType = null;
			List<DIO> doArgs = null;
			List<DIO> diArgs = null;
			if (AgvType.Roller.equals(type)) {
				if (operation.equalsIgnoreCase(JackOperationType.JackSetHeight.getOperation())) {
					onlyJackSetHeight = true;
					jackOperationType = JackOperationType.JackSetHeight;
					height = map.get("height").getDoubleEvaluatedValue();
				} else if (operation.toUpperCase().endsWith("DIO")) {
					rollByDIO = true;
					doArgs = JSONUtil.toList(map.get("doArgs").getEvaluatedValue(), DIO.class);
					diArgs = JSONUtil.toList(map.get("diArgs").getEvaluatedValue(), DIO.class);
					dioOperationType = DIOOperationType.getByOperation(operation);
					if (dioOperationType == null) {
						String exceptionMsg = StrUtil.format("invalid operation [%s]", operation);
						log.error(exceptionMsg);
						return CmdInstanceResult.exception(exceptionMsg);
					}
				}
				else {
					String side = map.get("side").getEvaluatedValue();
					rollerOperationType = RollerOperationType.getByOperationAndSide(operation, side);
					if (rollerOperationType == null) {
						String exceptionMsg = StrUtil.format("invalid operation [%s] or side [%s]", operation, side);
						log.error(exceptionMsg);
						return CmdInstanceResult.exception(exceptionMsg);
					}
				}
			}
			if (AgvType.Jack.equals(type)) {
				// 托盘车
				jackOperationType = JackOperationType.getByOperation(operation);
				if (jackOperationType == null) {
					String exceptionMsg = StrUtil.format("invalid operation [%s]", operation);
					log.error(exceptionMsg);
					return CmdInstanceResult.exception(exceptionMsg);
				}
			}

			if (agvMsg != null) {
				List<Nav> navs = JSONUtil.toList(agvMsg.getMsg(), Nav.class);
				List<String> taskIds = navs.stream().map(Nav::getTask_id).collect(Collectors.toList());
				// 查看发送的命令是否被agv执行完成；
				TaskStatusPackage taskStatusPackage = StatusService.getTaskStatus(agvVo.getIp(), taskIds);
				if (taskStatusPackage == null || CollUtil.isEmpty(taskStatusPackage.getTask_status_list())) {
					return CmdInstanceResult.retry();
				}
				List<Task> taskStatus = taskStatusPackage.getTask_status_list();
				// 故障处理：人工移动到目标点
				int uncompletedCnt = 0, completedCnt = 0, failedCnt = 0;
				for (Task task : taskStatus) {
					// 按照任务执行先后顺序存储
					Integer status = task.getStatus();
					if (status < 4) {
						// 未完成,uncompleted
						uncompletedCnt++;
					} else if (status == 4) {
						// 执行完成,completed
						completedCnt++;
					} else {
						// 执行失败,failed
						failedCnt++;
					}
				}
				// 默认到达站点成功
				if (completedCnt == taskIds.size()) {
					// 完全到达站点
					log.info("到达目标站点{}", targetStation);
					if (AgvType.Roller.equals(type)) {
						// 查看发送的命令是否被agv执行完成；
						// 增加判断是不是roller agv
						Roller roller = agvVo.getRoller();
						Jack jack = agvVo.getJack();
						if (onlyJackSetHeight) {
							// 只定高
							if (jack.setHeightSuccess(height)) {
								return CmdInstanceResult.success();
							}
							if (jack.isFailed()) {
								return CmdInstanceResult.retry(jack.getErr_msg());
							}
						} else if (rollByDIO) {
							// 是否需要确认di命令
							List<DIO> resetDOS = new ArrayList<>();
							for (DIO dio : doArgs) {
								DIO reset = new DIO();
								reset.setId(dio.getId());
								reset.setStatus(!dio.isStatus());
								resetDOS.add(reset);
							}
							// 重置do信号
							if (!MiscService.setDOS(agvVo.getIp(), resetDOS)) {
								return CmdInstanceResult.retry();
							}
							return CmdInstanceResult.success();
						} else {
							// 只辊筒
							if (rollerOperationType.isLoad()) {
								if (roller.loadSuccess()) {
									return CmdInstanceResult.success();
								}
								if (roller.isFailed()) {
									return CmdInstanceResult.retry(roller.getErr_msg());
								}
							}
							if (rollerOperationType.isUnload()) {
								if (roller.unloadSuccess()) {
									return CmdInstanceResult.success();
								}
								if (roller.isFailed()) {
									return CmdInstanceResult.retry(roller.getErr_msg());
								}
							}
						}
						return CmdInstanceResult.retry();
					} else if (AgvType.Jack.equals(type)) {
						// 托盘车
						Jack jack = agvVo.getJack();
						// 查看发送的命令是否被agv执行完成；
						if (jackOperationType.isLoad()) {
							// 定高成功
							if (jack.loadSuccess()) {
								return CmdInstanceResult.success();
							}
							if (jack.isFailed()) {
								return CmdInstanceResult.retry(jack.getErr_msg());
							}
						}
						if (jackOperationType.isUnload()) {
							// 定高成功
							if (jack.unloadSuccess()) {
								return CmdInstanceResult.success();
							}
							if (jack.isFailed()) {
								return CmdInstanceResult.retry(jack.getErr_msg());
							}
						}
						return CmdInstanceResult.retry();
					} else if (AgvType.Fork.equals(type)) {
						// 叉车
						Fork fork = agvVo.getFork();
						Double lowHeight = map.get("lowHeight").getDoubleEvaluatedValue();
						Double highHeight = map.get("highHeight").getDoubleEvaluatedValue();

					} else if (AgvType.Hook.equals(type)) {

					}
					return CmdInstanceResult.exception(String.format("invalid agv type = %s", type));
				} else if (completedCnt + uncompletedCnt == taskIds.size()) {
					return CmdInstanceResult.retry();
				} else {
					// failedCnt > 0
					// 重新发送导航命令
					log.info("导航失败，重新发送导航命令，failedCnt = {}", failedCnt);
					// 发送导航命令
					sendAndSaveAgvMsg(agvVo, cmdInstanceContext, targetStation, onlyOccupiedToCurrent, rollerOperationType, onlyJackSetHeight, jackOperationType, height, rollByDIO, dioOperationType, doArgs, diArgs);
					return CmdInstanceResult.retry();
				}
			} else {
				// 发送导航命令
				sendAndSaveAgvMsg(agvVo, cmdInstanceContext, targetStation, onlyOccupiedToCurrent, rollerOperationType, onlyJackSetHeight, jackOperationType, height, rollByDIO, dioOperationType, doArgs, diArgs);
				return CmdInstanceResult.retry();
			}
		} catch (Exception e) {
			log.error("AgvNavCmdExecutor execute error", e);
			return CmdInstanceResult.retry(e.getMessage());
		}
	}

	private void sendAndSaveAgvMsg(AgvVo agvVo, CmdInstanceContext cmdInstanceContext, String targetStation, boolean onlyOccupiedToCurrent, RollerOperationType rollerOperationType, boolean onlyJackSetHeight, JackOperationType jackOperationType, Double jackHeight, boolean rollByDIO, DIOOperationType dioOperationType, List<DIO> dos, List<DIO> dis) {
		List<BaseNav> navs = new ArrayList<>();
		if (targetStation.equalsIgnoreCase(RdsConfiguration.SelfPosition)) {
			// 原地不动

			String self_position = RdsConfiguration.SelfPosition;
			// 重新导航到当前点
			// 可能需要先到前置点然后再到当前点。查询相邻的点和车辆已完成点的交集，判断前置点，如果没有前置点无效直接成功；有前置点，先到前置点再到当前点。
			// 操作类的，更改为self_position
			AgvType type = agvVo.getType();
			if (AgvType.Roller.equals(type)) {
				if (onlyJackSetHeight) {
					NavJack navJack = new NavJack();
					navJack.setId(self_position);
					navJack.setSource_id(self_position);
					navJack.setTask_id(IdUtil.getSnowflakeNextIdStr());
					navJack.setOperation(jackOperationType.getOperationFull());
					navJack.setJack_height(jackHeight);
					navs.add(navJack);
				} else if (rollByDIO) {
					NavDO navDO = new NavDO();
					navDO.setId(self_position);
					navDO.setSource_id(self_position);
					navDO.setTask_id(IdUtil.getSnowflakeNextIdStr());
					navDO.setOperation(dioOperationType.getSetDOOperationFull());
					DOArgs doArgs = new DOArgs();
					doArgs.setDO(dos);
					navDO.setArgs(doArgs);
					navs.add(navDO);

					NavDI navDI = new NavDI();
					navDI.setId(RdsConfiguration.SelfPosition);
					navDI.setSource_id(RdsConfiguration.SelfPosition);
					navDI.setTask_id(IdUtil.getSnowflakeNextIdStr());
					navDI.setOperation(dioOperationType.getWaitDIOperationFull());
					DIArgs diArgs = new DIArgs();
					diArgs.setDI(dis);
					navDI.setArgs(diArgs);
					navs.add(navDI);
				} else {
					NavRoller navRoller = new NavRoller();
					navRoller.setId(self_position);
					navRoller.setSource_id(self_position);
					navRoller.setTask_id(IdUtil.getSnowflakeNextIdStr());
					navRoller.setOperation(rollerOperationType.getOperationFull());
					navRoller.setDirection(rollerOperationType.getSide());
					navs.add(navRoller);
				}
			} else if (AgvType.Jack.equals(type)) {
				NavJack navJack = new NavJack();
				navJack.setId(self_position);
				navJack.setSource_id(self_position);
				navJack.setTask_id(IdUtil.getSnowflakeNextIdStr());
				navJack.setOperation(jackOperationType.getOperationFull());
				if (jackHeight != null) navJack.setJack_height(jackHeight);
				navs.add(navJack);
			} else if (AgvType.Fork.equals(type)) {
				// ni
			} else if (AgvType.Hook.equals(type)) {
				// ni
			}
		} else {
			//导航+动作
			// 获取导航路径
			List<String> paths = NavService.getNavPath(agvVo.getIp(), agvVo.getCurrentStation(), targetStation);
			if (CollUtil.isEmpty(paths)) {
				return;
			}
			// 检查交管是否满足
			if (!TrafficManagement.tryTraffic(onlyOccupiedToCurrent, cmdInstanceContext, agvVo, targetStation, paths)) return;

			// 发送导航命令 // 可以记录task_id，但每次重新发送重新生成task_id
			for (int i = 1; i < paths.size(); i++) {
				if (i == paths.size() - 1) {
					// 最后一个节点
					AgvType type = agvVo.getType();
					if (AgvType.Roller.equals(type)) {
						if (onlyJackSetHeight) {
							NavJack navJack = new NavJack();
							navJack.setId(paths.get(i));
							navJack.setSource_id(paths.get(i - 1));
							navJack.setTask_id(IdUtil.getSnowflakeNextIdStr());
							navJack.setOperation(jackOperationType.getOperationFull());
							navJack.setJack_height(jackHeight);
							navs.add(navJack);
						} else if (rollByDIO) {
							NavDO navDO = new NavDO();
							navDO.setId(paths.get(i));
							navDO.setSource_id(paths.get(i - 1));
							navDO.setTask_id(IdUtil.getSnowflakeNextIdStr());
							navDO.setOperation(dioOperationType.getSetDOOperationFull());
							DOArgs doArgs = new DOArgs();
							doArgs.setDO(dos);
							navDO.setArgs(doArgs);
							navs.add(navDO);

							NavDI navDI = new NavDI();
							navDI.setId(RdsConfiguration.SelfPosition);
							navDI.setSource_id(RdsConfiguration.SelfPosition);
							navDI.setTask_id(IdUtil.getSnowflakeNextIdStr());
							navDI.setOperation(dioOperationType.getWaitDIOperationFull());
							DIArgs diArgs = new DIArgs();
							diArgs.setDI(dis);
							navDI.setArgs(diArgs);
							navs.add(navDI);
						} else {
							NavRoller navRoller = new NavRoller();
							navRoller.setId(paths.get(i));
							navRoller.setSource_id(paths.get(i - 1));
							navRoller.setTask_id(IdUtil.getSnowflakeNextIdStr());
							navRoller.setOperation(rollerOperationType.getOperationFull());
							navRoller.setDirection(rollerOperationType.getSide());
							navs.add(navRoller);
						}
					} else if (AgvType.Jack.equals(type)) {
						NavJack navJack = new NavJack();
						navJack.setId(paths.get(i));
						navJack.setSource_id(paths.get(i - 1));
						navJack.setTask_id(IdUtil.getSnowflakeNextIdStr());
						navJack.setOperation(jackOperationType.getOperationFull());
						if (jackHeight != null) navJack.setJack_height(jackHeight);
						navs.add(navJack);
					} else if (AgvType.Fork.equals(type)) {
						// ni
					} else if (AgvType.Hook.equals(type)) {
						// ni
					}
				} else {
					Nav nav = new Nav();
					nav.setId(paths.get(i));
					nav.setSource_id(paths.get(i - 1));
					nav.setTask_id(IdUtil.getSnowflakeNextIdStr());
					navs.add(nav);
				}
			}
		}

		boolean success = NavService.goTargetList(agvVo.getIp(), navs);
		// 发送导航命令成功
		if (success) {
			// 记录导航命令
			AgvMsg agvMsg = new AgvMsg();
			agvMsg.setId(IdUtil.getSnowflakeNextIdStr());
			agvMsg.setCmdInstanceId(cmdInstanceContext.getCmdInstanceId());
			agvMsg.setWaybillInstanceId(cmdInstanceContext.getWaybillInstanceId());
			agvMsg.setMsg(JSONUtil.toJsonStr(navs));
			Date now = new Date();
			agvMsg.setCreateTime(now);
			agvMsg.setCreateBy(UserConstants.SYS_USER);
			agvMsg.setUpdateTime(now);
			agvMsg.setUpdateBy(UserConstants.SYS_USER);
			agvMsgService.insert(agvMsg);
			cmdInstanceContext.setLastAgvMsg(agvMsg);
		}
	}
}
